﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.IO;
using System.Diagnostics;

namespace MotorGUI
{
    public partial class MainForm : Form
    {
        static uint canHandle;
        Thread RXThread;
        bool RXActive;
        const int CANBaseAddr = 0x0005000;
        private Waveform waveForm1 = new Waveform();
        private Waveform waveForm2 = new Waveform();

        // data
        SDataStructure DataStructure = new SDataStructure();

        public struct SDataStructure
        {
            public int ReceivedPackets;
            public int ReceiveErrors;
            public int RXThreadTicks;

            public int TransmitPackets;
            public int TransmitErrors;

            // data
            // ESC-X1 Data
            public int StateActual;
            public int ModeActual;
            public float CurrentId;
            public float CurrentIq;
            public float SpeedRPM;
            public float PositionN;
            public float BattVoltage;
            public float Temperature;
            public float LoopTime;
            public float LoopTimeMAX;
            public float INTLoopTime;
            public float INTLoopTimeMAX;
            public double TotalTime;
        };

        public MainForm()
        {
            InitializeComponent();
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            toolStripStatusLabelCANAdapter.Text = "No adapters";

            // find adapters
            StringBuilder buf = new StringBuilder(32);
            int cnt;

            // Fill the listbox with data
            cnt = LAWICEL.canusb_getFirstAdapter(buf, 32);
            if (cnt > 0) toolStripStatusLabelCANAdapter.Text = "Adapter found: " + buf.ToString();

            // wavefrm init
            waveForm1.Init(30, 30, Color.Green, Color.LightSkyBlue);
            waveForm2.Init(10000, 10000, Color.Yellow, Color.Red);
        }

        private void buttonOpen_Click(object sender, EventArgs e)
        {
            canHandle = LAWICEL.canusb_Open(IntPtr.Zero,
                                        LAWICEL.CAN_BAUD_1M,
                                        LAWICEL.CANUSB_ACCEPTANCE_CODE_ALL,
                                        LAWICEL.CANUSB_ACCEPTANCE_MASK_ALL,
                                        0);

            if (canHandle == 0) MessageBox.Show("Open Failed");
            else
            {
                // OK
                buttonOpen.Enabled = false;
                buttonClose.Enabled = true;

                // init structures
                DataStructure = new SDataStructure();

                // create receive thread
                RXActive = true;
                RXThread = new Thread(new ThreadStart(RXThreadProc));
                //RXThread.Priority = ThreadPriority.AboveNormal;
                RXThread.IsBackground = true;
                RXThread.Start();

                timerGUI.Enabled = true;
            }
        }

        private void buttonClose_Click(object sender, EventArgs e)
        {
            timerGUI.Enabled = false;

            // Close the CANUSB
            int res = LAWICEL.canusb_Close(canHandle);
            if (LAWICEL.ERROR_CANUSB_OK == res)
            {
                // OK
                buttonOpen.Enabled = true;
                buttonClose.Enabled = false;
                RXActive = false;
            }
            else
            {
                MessageBox.Show("Close Failed");
            }
        }

        void RXThreadProc()
        {
            // open log file
            StreamWriter logFile = File.CreateText(Application.StartupPath + "\\LogFile.txt");
            logFile.WriteLine("Timestamp ID Data");
            Stopwatch timer = new Stopwatch();
            timer.Start();

            while (RXActive)
            {
                // Read one CAN message
                LAWICEL.CANMsg msg = new LAWICEL.CANMsg();

                int rv = LAWICEL.canusb_Read(canHandle, out msg);
                if (LAWICEL.ERROR_CANUSB_OK == rv)
                {
                    // data OK, convert to float
                    byte[] bytes = BitConverter.GetBytes(msg.data);
                    float data = BitConverter.ToSingle(bytes, 0);
                    double dataDbl = BitConverter.ToDouble(bytes, 0);

                    lock (this)
                    {
                        // ESC X-1
                        uint baseAddr = CANBaseAddr;
                        if (msg.id == 0x300 + baseAddr) DataStructure.StateActual = (int)data;
                        if (msg.id == 0x301 + baseAddr) DataStructure.ModeActual = (int)data;
                        if (msg.id == 0x302 + baseAddr) DataStructure.CurrentId = data;
                        if (msg.id == 0x303 + baseAddr) DataStructure.CurrentIq = data;
                        if (msg.id == 0x304 + baseAddr) DataStructure.SpeedRPM = data;
                        if (msg.id == 0x305 + baseAddr) DataStructure.PositionN = data;
                        if (msg.id == 0x310 + baseAddr) DataStructure.BattVoltage = data;
                        if (msg.id == 0x311 + baseAddr) DataStructure.Temperature = data;
                        if (msg.id == 0x350 + baseAddr) DataStructure.LoopTime = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.TotalTime = dataDbl;
                        if (msg.id == 0x352 + baseAddr) DataStructure.LoopTimeMAX = data;
                        if (msg.id == 0x353 + baseAddr) DataStructure.INTLoopTime = data;
                        if (msg.id == 0x354 + baseAddr) DataStructure.INTLoopTimeMAX = data;

                        DataStructure.ReceivedPackets++;
                    }

                    // log data
                    double currentMS = (double)timer.ElapsedTicks/(double)Stopwatch.Frequency * 1000.0;
                    // Fast logging
                    if (msg.id == 0x500 + CANBaseAddr)
                    {
                        // convert in postprocessing!
                        Int16 data1 = BitConverter.ToInt16(bytes, 0);
                        Int16 data2 = BitConverter.ToInt16(bytes, 2);
                        Int16 data3 = BitConverter.ToInt16(bytes, 4);
                        Int16 data4 = BitConverter.ToInt16(bytes, 6);

                        string line = string.Format("{0} {1} {2} {3} {4}", currentMS, data1, data2, data3, data4);
                        logFile.WriteLine(line);

                        // send to waveforms
                        SWFData[] newData = new SWFData[1];
                        newData[0].D1 = data1/300.0;
                        newData[0].D2 = data2 / 300.0;
                        waveForm1.AddPoints(newData);
                        newData[0].D1 = data3;
                        newData[0].D2 = data4;
                        waveForm2.AddPoints(newData);
                    }

                    // Slow logging
                    /*if (msg.id == 0x351 + CANBaseAddr)
                    {
                        // double
                        string line = string.Format("{0} {1} {2}", currentMS, msg.id - CANBaseAddr - 0x300, dataDbl);
                        logFile.WriteLine(line);
                    }
                    else
                    {
                        // float
                        string line = string.Format("{0} {1} {2}", currentMS, msg.id - CANBaseAddr - 0x300, data);
                        logFile.WriteLine(line);
                    }*/
                }
                else if (LAWICEL.ERROR_CANUSB_NO_MESSAGE == rv)
                {
                    // no messages, sleep
                    Thread.Sleep(1);
                    lock (this)
                    {
                        DataStructure.RXThreadTicks++;
                    }
                }

                else
                {
                    // error!
                    lock (this)
                    {
                        DataStructure.ReceiveErrors++;
                    }
                }
            }

            logFile.Close();
        }

        public void SendCANMessage(uint id, float testVar)
        {
            // Send one message with extended id
            LAWICEL.CANMsg msg = new LAWICEL.CANMsg();
            msg.id = id;
            msg.len = 8;
            msg.flags = LAWICEL.CANMSG_EXTENDED;
            byte[] data = new byte[8];
            BitConverter.GetBytes(testVar).CopyTo(data, 0);
            msg.data = BitConverter.ToUInt64(data, 0);
            int rv = LAWICEL.canusb_Write(canHandle, ref msg);
            if (LAWICEL.ERROR_CANUSB_OK == rv)
            {
                // OK
                lock (this)
                {
                    DataStructure.TransmitPackets++;
                }
            }
            else if (LAWICEL.ERROR_CANUSB_TX_FIFO_FULL == rv)
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
            else
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
        }

        private void timerGUI_Tick(object sender, EventArgs e)
        {
            // update GUI
            // 1. get copy
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }

            // 2. set values
            textBoxTXRXFrames.Text = string.Format("{0}/{1}", copyStruct.TransmitPackets, copyStruct.ReceivedPackets);
            textBoxTXRXErrors.Text = string.Format("{0}/{1}", copyStruct.TransmitErrors, copyStruct.ReceiveErrors);
            textBoxRXTicks.Text = copyStruct.RXThreadTicks.ToString();


            textBoxStateActual.Text = string.Format("{0}", copyStruct.StateActual);
            textBoxModeActual.Text = string.Format("{0}", copyStruct.ModeActual);
            textBoxCurrentId.Text = string.Format("{0:f1} A", copyStruct.CurrentId);
            textBoxCurrentIq.Text = string.Format("{0:f1} A", copyStruct.CurrentIq);
            textBoxSpeed.Text = string.Format("{0:f1} rpm", copyStruct.SpeedRPM);
            textBoxPosition.Text = string.Format("{0:f1} Pulses", copyStruct.PositionN);
            textBoxBattVoltage.Text = string.Format("{0:f1} V", copyStruct.BattVoltage);
            textBoxTemperature.Text = string.Format("{0:f1} °C", copyStruct.Temperature);
            textBoxLoopMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTime);
            textBoxLoopMAXMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTimeMAX);
            textBoxINTLoop.Text = string.Format("{0:f2} us", copyStruct.INTLoopTime);
            textBoxINTLoopMAX.Text = string.Format("{0:f2} us", copyStruct.INTLoopTimeMAX);
            textBoxTotalTime.Text = string.Format("{0:f2} s", copyStruct.TotalTime);

            // update mode
            int mode = (int)copyStruct.ModeActual;
            switch (mode)
            {
                case 0: //off
                    pictureOff.BackColor = Color.Lime;
                    pictureOLDuty.BackColor = Color.Red;
                    pictureOLCurrent.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    SetMinMaxVRef(false, -0.1, 0.1);
                    SetMinMaxIRef(false, -0.3, 0.3);
                    SetMinMaxRPMRef(false, -400, 400);
                    buttonOff.Enabled = true;
                    buttonOLDuty.Enabled = true;
                    buttonOLCurrent.Enabled = true;
                    buttonTorque.Enabled = true;
                    buttonSpeed.Enabled = true;
                    buttonPosition.Enabled = true;
                    break;

                case 1: // OLDuty
                    pictureOff.BackColor = Color.Red;
                    pictureOLDuty.BackColor = Color.Lime;
                    pictureOLCurrent.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    SetMinMaxVRef(true, -0.1, 0.1);
                    SetMinMaxIRef(false, -0.3, 0.3);
                    SetMinMaxRPMRef(true, -400, 400);
                    buttonOff.Enabled = true;
                    buttonOLDuty.Enabled = true;
                    buttonOLCurrent.Enabled = false;
                    buttonTorque.Enabled = false;
                    buttonSpeed.Enabled = false;
                    buttonPosition.Enabled = false;
                    break;

                case 2: // OLCurrent
                    pictureOff.BackColor = Color.Red;
                    pictureOLDuty.BackColor = Color.Red;
                    pictureOLCurrent.BackColor = Color.Lime;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    SetMinMaxVRef(false, -0.1, 0.1);
                    SetMinMaxIRef(true, -0.3, 0.3);
                    SetMinMaxRPMRef(true, -400, 400);
                    buttonOff.Enabled = true;
                    buttonOLDuty.Enabled = false;
                    buttonOLCurrent.Enabled = true;
                    buttonTorque.Enabled = false;
                    buttonSpeed.Enabled = false;
                    buttonPosition.Enabled = false;
                    break;

                case 3: // Torque
                    pictureOff.BackColor = Color.Red;
                    pictureOLDuty.BackColor = Color.Red;
                    pictureOLCurrent.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Lime;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    SetMinMaxVRef(false, -0.1, 0.1);
                    SetMinMaxIRef(true, -0.25, 0.25);
                    SetMinMaxRPMRef(false, -3000, 3000);
                    buttonOff.Enabled = true;
                    buttonOLDuty.Enabled = false;
                    buttonOLCurrent.Enabled = false;
                    buttonTorque.Enabled = true;
                    buttonSpeed.Enabled = false;
                    buttonPosition.Enabled = false;
                    break;

                case 4: // Speed
                    pictureOff.BackColor = Color.Red;
                    pictureOLDuty.BackColor = Color.Red;
                    pictureOLCurrent.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Lime;
                    picturePosition.BackColor = Color.Red;
                    SetMinMaxVRef(false, -0.1, 0.1);
                    SetMinMaxIRef(false, -0.3, 0.3);
                    SetMinMaxRPMRef(true, -12000, 12000);
                    buttonOff.Enabled = true;
                    buttonOLDuty.Enabled = false;
                    buttonOLCurrent.Enabled = false;
                    buttonTorque.Enabled = false;
                    buttonSpeed.Enabled = true;
                    buttonPosition.Enabled = false;
                    break;

                case 5: // Position
                    pictureOff.BackColor = Color.Red;
                    pictureOLDuty.BackColor = Color.Red;
                    pictureOLCurrent.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Lime;
                    SetMinMaxVRef(false, -0.1, 0.1);
                    SetMinMaxIRef(true, -0.3, 0.3);
                    SetMinMaxRPMRef(false, -3000, 3000);
                    buttonOff.Enabled = true;
                    buttonOLDuty.Enabled = false;
                    buttonOLCurrent.Enabled = false;
                    buttonTorque.Enabled = false;
                    buttonSpeed.Enabled = false;
                    buttonPosition.Enabled = true;
                    break;

                default:
                    pictureOff.BackColor = Color.Red;
                    pictureOLDuty.BackColor = Color.Red;
                    pictureOLCurrent.BackColor = Color.Red;
                    pictureTorque.BackColor = Color.Red;
                    pictureSpeed.BackColor = Color.Red;
                    picturePosition.BackColor = Color.Red;
                    SetMinMaxVRef(false, -0.1, 0.1);
                    SetMinMaxIRef(false, -0.3, 0.3);
                    SetMinMaxRPMRef(false, -3000, 3000);
                    break;
            }

            // update status
            int status = copyStruct.StateActual;
            switch (status)
            {
                case 0:
                    pictureStatus.BackColor = Color.DimGray;
                    buttonGoOffline.Enabled = false;
                    buttonGoOnline.Enabled = true;
                    buttonAckError.Enabled = false;
                    break;
                case 1:
                    pictureStatus.BackColor = Color.Lime;
                    buttonGoOffline.Enabled = true;
                    buttonGoOnline.Enabled = false;
                    buttonAckError.Enabled = false;
                    break;
                case 2:
                    pictureStatus.BackColor = Color.Red;
                    buttonGoOffline.Enabled = false;
                    buttonGoOnline.Enabled = false;
                    buttonAckError.Enabled = true;
                    break;
            }

            // 3. read inputs
            float VRef = (float)GetVRef();
            float IRef = (float)GetIRef();
            float RPMRef = (float)GetRPMRef();

            // 4. send messages
            bool silentMode = false;
            if (!silentMode)
            {
                uint baseAddr = CANBaseAddr;
                SendCANMessage(0x0110 + baseAddr, VRef);
                SendCANMessage(0x0111 + baseAddr, IRef);
                SendCANMessage(0x0112 + baseAddr, RPMRef);
                //SendCANMessage(0x0113 + baseAddr, PosRef);
            }

            // 5. Read status (reset error???)
            //int bla = LAWICEL.canusb_Status(canHandle);

            // draw waveforms
            waveForm1.Draw(panelWaveform1);
            waveForm2.Draw(panelWaveform2);
        }
        double T = 0;

        private void buttonModeSelect_Click(object sender, EventArgs e)
        {
            uint modeAddress = CANBaseAddr + 0x0101;

            if (sender == buttonOff)
            {
                ResetRefs();
                SendCANMessage(modeAddress, 0);
            }
            else if (sender == buttonOLDuty) SendCANMessage(modeAddress, 1);
            else if (sender == buttonOLCurrent) SendCANMessage(modeAddress, 2);
            else if (sender == buttonTorque) SendCANMessage(modeAddress, 3);
            else if (sender == buttonSpeed) SendCANMessage(modeAddress, 4);
            else if (sender == buttonPosition) SendCANMessage(modeAddress, 5);
        }

        private void buttonStateSelect_Click(object sender, EventArgs e)
        {
            uint stateAddress = CANBaseAddr + 0x0100;
            uint ackErrorAddress = CANBaseAddr + 0x0102;

            if (sender == buttonGoOffline) SendCANMessage(stateAddress, 0);
            else if (sender == buttonGoOnline) SendCANMessage(stateAddress, 1);
            else if (sender == buttonAckError)
            {
                ResetRefs();
                SendCANMessage(ackErrorAddress, 1);
            }
        }

        private void ResetRefs()
        {
            // reset Refs
            trackBarVRef.Value = 0;
            trackBarIRef.Value = 0;
            trackBarRPMRef.Value = 0;
        }

        private double VALVRefMin = 0;
        private double VALVRefMax = 0;
        private double VALIRefMin = 0;
        private double VALIRefMax = 0;
        private double VALRPMRefMin = 0;
        private double VALRPMRefMax = 0;
        // VRef
        private void SetMinMaxVRef(bool enable, double min, double max)
        {
            if (enable == true)
            {
                trackBarVRef.Enabled = true;
                textBoxVRefMin.Enabled = true;
                textBoxVRefVal.Enabled = true;
                textBoxVRefMax.Enabled = true;
                trackBarVRef.Minimum = -1000;
                trackBarVRef.Maximum = 1000;
                textBoxVRefMin.Text = min.ToString();
                textBoxVRefMax.Text = max.ToString();

                VALVRefMin = min;
                VALVRefMax = max;
            }
            else
            {
                trackBarVRef.Enabled = false;
                textBoxVRefMin.Enabled = false;
                textBoxVRefVal.Enabled = false;
                textBoxVRefMax.Enabled = false;
            }
        }
        private double GetVRef()
        {
            double refValue = (double)trackBarVRef.Value / (double)trackBarVRef.Maximum;
            refValue = refValue * VALVRefMax;
            textBoxVRefVal.Text = refValue.ToString();

            return refValue;
        }
        private void textBoxVRefVal_Click(object sender, EventArgs e)
        {
            trackBarVRef.Value = 0;
        }

        // IRef
        private void SetMinMaxIRef(bool enable, double min, double max)
        {
            if (enable == true)
            {
                trackBarIRef.Enabled = true;
                textBoxIRefMin.Enabled = true;
                textBoxIRefVal.Enabled = true;
                textBoxIRefMax.Enabled = true;
                trackBarIRef.Minimum = -1000;
                trackBarIRef.Maximum = 1000;
                textBoxIRefMin.Text = min.ToString();
                textBoxIRefMax.Text = max.ToString();

                VALIRefMin = min;
                VALIRefMax = max;
            }
            else
            {
                trackBarIRef.Enabled = false;
                textBoxIRefMin.Enabled = false;
                textBoxIRefVal.Enabled = false;
                textBoxIRefMax.Enabled = false;
            }
        }
        private double GetIRef()
        {
            double refValue = (double)trackBarIRef.Value / (double)trackBarIRef.Maximum;
            refValue = refValue * VALIRefMax;
            textBoxIRefVal.Text = refValue.ToString();

            return refValue;
        }
        private void textBoxIRefVal_Click(object sender, EventArgs e)
        {
            trackBarIRef.Value = 0;
        }

        // RPM
        private void SetMinMaxRPMRef(bool enable, double min, double max)
        {
            if (enable == true)
            {
                trackBarRPMRef.Enabled = true;
                textBoxRPMRefMin.Enabled = true;
                textBoxRPMRefVal.Enabled = true;
                textBoxRPMRefMax.Enabled = true;
                trackBarRPMRef.Minimum = -1000;
                trackBarRPMRef.Maximum = 1000;
                textBoxRPMRefMin.Text = min.ToString();
                textBoxRPMRefMax.Text = max.ToString();

                VALRPMRefMin = min;
                VALRPMRefMax = max;
            }
            else
            {
                trackBarRPMRef.Enabled = false;
                textBoxRPMRefMin.Enabled = false;
                textBoxRPMRefVal.Enabled = false;
                textBoxRPMRefMax.Enabled = false;
            }
        }
        private double GetRPMRef()
        {
            double refValue = (double)trackBarRPMRef.Value / (double)trackBarRPMRef.Maximum;
            refValue = refValue * VALRPMRefMax;
            textBoxRPMRefVal.Text = refValue.ToString();

            return refValue;
        }
        private void textBoxRPMRefVal_Click(object sender, EventArgs e)
        {
            trackBarRPMRef.Value = 0;
        }
    }
}
